/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/trajectory_navigation.cpp
 * @Description  : 实现了AGV的轨迹导航系统。
 *                 主要功能包括初始化和创建不同类型的轨迹对象（如直线、贝塞尔曲线、侧向轨迹等），
 *                 管理导航过程中的速度规划和转向控制，检查路径丢失和偏差，并根据导航类型（如二维码、激光、磁导航等）
 *                 生成相应的导航速度和角速度。代码中包含了路径验证、车辆状态更新、辅助轨迹生成器的创建和启动等功能。
 *                 还处理了导航过程中可能出现的紧急停止、缓慢停止、暂停等控制信号。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:54:06
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "trajectory_navigation.h" 
#include "status.h"
// #include "TaskMan.h"
#include "public_kernel.h"
#include "check_manager.h"

extern World world;
extern Timer timer;
extern WorkStatusRecord VhcStatus;

bool TrajNv::init_posture_known_ = FALSE;
Posture TrajNv::static_posture_ = Posture(); // Current posture
//////////////////////////////////////////////////////////////////////////////
/**
 * @brief TrajNv 类的构造函数
 *
 * 初始化 TrajNv 对象，并根据路径类型创建相应的轨迹对象。
 *
 * @param path 路径对象指针
 * @param to_node 目标节点引用
 * @param from 起始位置参数
 * @param to 目标位置参数
 * @param level 层级参数
 * @param force_check_mark 是否强制检查标记
 * @param use_speed_level 是否使用速度层级
 */
TrajNv::TrajNv(Path *path, Node &to_node, float from, 
    float to, short level, bool force_check_mark, 
    bool use_speed_level, bool use_temp_end_point)
{
    max_dead_distance_ = 0;
    guide_type_ = 1;
    ignore_y_error_ = false;
    traj_pointer_ = nullptr;
    spin_node_pointer_ = nullptr;
    path_pointer_ = path;
    force_check_mark_ = force_check_mark;
    use_speed_level_ = use_speed_level;
    level_ = level;
    to_node_ = to_node;
    use_temp_end_point_ = use_temp_end_point;

    if (!path_pointer_) {
        DEBUG_ERROR_OUT("error: cur path is null");
        return;
    }

    // According to the path of the map to
    USHORT guide_type = path_pointer_->guide_type_;
    Point path_start_point = path_pointer_->GetStartPoint();
    Point path_end_point = path_pointer_->GetEndPoint();
    MoveDir move_dir = path_pointer_->move_dir_; // FORWARD;
    start_node_id_ = path_pointer_->start_node_;
    end_node_id_ = path_pointer_->end_node_;

    // move here  by yxj
    Node *end_node = world.GetNode(end_node_id_);
    if (!end_node) {
        DEBUG_ERROR_OUT("error: cur end_node_point is null, end_node_id_=" << end_node_id_);
        return;
    }
    float sure_area = end_node ? end_node->chk_mark_dist_ : 0.0f;
    sure_area = sure_area * 2;

    // 货物状态
    bool load = MotionParm::load_flags_.current_load;
    float limit_velocity = load ? path_pointer_->velocity_limit_load_ : path_pointer_->velocity_limit_;
    DEBUG_OUT("" << ",load=" << (int)(load)
                 << ",limit_velocity=" << limit_velocity
                 << ",loadVel=" << path_pointer_->velocity_limit_load_
                 << ",unloadVel=" << path_pointer_->velocity_limit_);

    float search_0_value = __min_(end_node->chk_mark_vel_, limit_velocity);

    //  Create trajectory object according to the path type
    //  MoveDir:  mainly  for turnPattern and sppshift(arc.eg)
    //  angHeading mainly for scp lazyS side need angHeading
    switch (path_pointer_->type_) {
        case (int)PathType::LINE_TYPE:
        {
            // #2 new line traj
            traj_pointer_ = new LineTrajectory;
            if (traj_pointer_ == nullptr)
                return;

            DEBUG_OUT("line traj:startPnt.x="
                      << path_start_point.x_ << ",y=" << path_start_point.y_
                      << ",endPnt.x=" << path_end_point.x_
                      << ",y=" << path_end_point.y_);

            ((LineTrajectory *)traj_pointer_)->CreateTrajectory(path_start_point, path_end_point, move_dir, from, to);
        }
        break;

        case (int)PathType::BEZIER_TYPE:
        {
            traj_pointer_ = new BezierTrajectory;
            if (traj_pointer_ == nullptr)
                return;
            BezierPath *bezier_path = static_cast<BezierPath *>(path_pointer_);

            if (!bezier_path) {
                DEBUG_ERROR_OUT("error: bezier_path is null");
                return;
            }

            bool shift_type = bezier_path->IsShift();
            Angle heading = bezier_path->GetHeading(Node());
            Point ctrl_point_1 = bezier_path->GetCtrlPoint1();
            Point ctrl_point_2 = bezier_path->GetCtrlPoint2();

            if (use_temp_end_point_) {
                path_end_point.x_ = to_node.x_;
                path_end_point.y_ = to_node.y_;
            }

            DEBUG_OUT("bezier traj:startPnt.x="
                      << path_start_point.x_ << ",y=" << path_start_point.y_
                      << ", ctrlPnt1.x=" << ctrl_point_1.x_ << ",y=" << ctrl_point_1.y_
                      << ", ctrlPnt2.x=" << ctrl_point_2.x_ << ",y=" << ctrl_point_2.y_
                      << ", endPnt.x=" << path_end_point.x_
                      << ",y=" << path_end_point.y_);

            ((BezierTrajectory *)traj_pointer_)->CreateTrajectory(path_start_point, ctrl_point_1, ctrl_point_2, path_end_point, move_dir, shift_type, heading, from, to);
        }
        break;

        case (int)PathType::SIDE_TYPE:
        {
            traj_pointer_ = new SideTrajectory;
            if (traj_pointer_ == nullptr)
                return;
            // important: use map's side path's m_angHeading
            Angle heading = path_pointer_->GetHeading(Node());
            ((SideTrajectory *)traj_pointer_)->CreateTrajectory(path_start_point, path_end_point, heading, from, to);
        }
        break;
    }

    // Get the navigation parameters for linear movement
    float max_velocity = LinearCruiseVel();
    float refrain_velocity = LinearRefrainVel();
    float up_acc = MotionParm::up_acc_;
    float down_acc = -MotionParm::down_acc_;
    float up_small_acc = MotionParm::up_low_acc_;
    float down_small_acc = MotionParm::down_low_acc_;
    float transmit_velocity = MotionParm::transmit_vel_;

    float traj_max_velocity = traj_pointer_->GetProfileValue(max_velocity);
    float traj_refrain_velocity = traj_pointer_->GetProfileValue(refrain_velocity);
    float up_slope_acc = traj_pointer_->GetProfileSlope(up_acc);
    float down_slope_acc = traj_pointer_->GetProfileSlope(down_acc);
    float up_small_slope_acc = traj_pointer_->GetProfileSlope(up_small_acc);
    float down_small_slope_acc = traj_pointer_->GetProfileSlope(down_small_acc);

    // mark
    int is_mark_node= to_node_.IsMarkNode();

    Create(traj_max_velocity, traj_refrain_velocity, up_slope_acc, down_slope_acc, up_small_slope_acc, down_small_slope_acc, transmit_velocity,
           sure_area, search_0_value, guide_type, is_mark_node, path_pointer_->max_dead_dist_);
}

/**
 * @brief TrajNv类的构造函数
 *
 * 用于初始化TrajNv对象，并创建相应的旋转轨迹。
 *
 * @param node 指向Node的指针，表示旋转的节点
 * @param ang_start 旋转开始的角度（Angle类型）
 * @param ang_end 旋转结束的角度（Angle类型）
 * @param level 旋转的级别（short类型）
 * @param sync_type 同步类型（bool类型）
 * @param turn_dir 旋转方向（int类型）
 * @param use_speed_level 是否使用速度级别（bool类型）
 * @param check_mark 是否检查地标（bool类型）
 * @param area 地标检测区域大小（float类型）
 */
TrajNv::TrajNv(Node *node, Angle &ang_start, Angle &ang_end, short level, 
                bool sync_type, int turn_dir, bool use_speed_level, bool check_mark, float area)
{
    traj_pointer_ = nullptr;
    spin_node_pointer_ = nullptr;
    if (!node) {
        DEBUG_ERROR_OUT("error: spin_node_pointer_ is null");
        return;
    }
    spin_node_pointer_ = node;
    path_pointer_ = nullptr;
    use_speed_level_ = use_speed_level;
    level_ = level;
    force_check_mark_ = FALSE;
    start_node_id_ = end_node_id_ = spin_node_pointer_ ? spin_node_pointer_->id_ : 0;
    float check_mark_area = 0.0f;
    Angle ang_sure_turn(0.0f);
    Angle ang_sure_end(0.0f);

    traj_pointer_ = new SpinTrajectory;
    // ASSERT(traj_pointer_ != NULL);
    if (traj_pointer_ == nullptr) return;

    to_node_ = *node;
    to_node_.heading_ = ang_end.radian_;        // FIXME:to_node_.m_fHeading = angEnd.Degree();

    // smart to judge turn dir
    TurnDir judge_turn_dir;
    if (turn_dir == SMART_TURN_DIR) {           // always true SMART_TURN_DIR
        Angle ang_turn = ang_end - ang_start;   // end - start
        judge_turn_dir = (ang_turn.Quadrant() <= 2) ? TurnDirTag::CounterClockwise : TurnDirTag::Clockwise;
    } else {
        judge_turn_dir = (turn_dir == 0) ? TurnDirTag::CounterClockwise : TurnDirTag::Clockwise;
    }

    DEBUG_OUT("spin turn dir=" << (int)judge_turn_dir.turn_dir_tag_);

    auto spin_params = MotionParm::spin_params_;
    // Check the mark area in spin turn mode
    // 不检测地标 此逻辑不会执行
    if (check_mark) {
        if (area < 0 || area > 50) {
            area = 15;
        }

        area += 3.0f;
        check_mark_area = TO_RADIAN(area);
        bool is_anti_clock = judge_turn_dir == TurnDirTag::CounterClockwise;
        ang_sure_end = is_anti_clock ? (ang_end + check_mark_area) : (ang_end - check_mark_area);
        ang_sure_turn = ang_sure_end - ang_start;
        ang_sure_turn = is_anti_clock ? ang_sure_turn : -ang_sure_turn;
    } else {
        // 正常执行逻辑，增加10度的开放段
        // 旋转时，设置10度开放段，提前减速
        spin_params.remain_angle_ = __max_(spin_params.remain_angle_, 5.0f);
        spin_params.remain_angle_ = __min_(spin_params.remain_angle_, 60.0f); // 1.0
        check_mark_area = TO_RADIAN(spin_params.remain_angle_);
        // fChkMarkArea = TO_RADIAN(10);
    }

    Point &node_pointer = spin_node_pointer_->GetPntObject();
    if (check_mark)
        ((SpinTrajectory *)traj_pointer_)->CreateTrajectory(node_pointer, ang_start, ang_end, judge_turn_dir, 0.0f, ang_sure_turn.radian_);
    else
        ((SpinTrajectory *)traj_pointer_)->CreateTrajectory(node_pointer, ang_start, ang_end, judge_turn_dir);

    // set max spin vel
    // no guide
    if (!spin_node_pointer_) {
        DEBUG_ERROR_OUT("error:spin_node is null");
    }

    // 货物状态
    bool load = MotionParm::load_flags_.current_load;
    float max_velocity = load ? spin_node_pointer_->max_spin_load_ : spin_node_pointer_->max_spin_value_;
    float up_slope_acc = load ? spin_node_pointer_->spin_acc_load_ : spin_node_pointer_->spin_acc_value_;
    float down_slope_acc = load ? spin_node_pointer_->spin_dcc_load_ : spin_node_pointer_->spin_dcc_value_;
    DEBUG_OUT("spin:CMapBasedNv:" << ",load=" << (int)(load)
                                  << ",max_velocity=" << max_velocity
                                  << ",up_slope_acc=" << up_slope_acc
                                  << ",down_slope_acc=" << down_slope_acc);

    // float fMaxValue = spin_node_pointer_ ? spin_node_pointer_->m_fMaxSpinValue : 0.45f;
    // float fUpSlope = spin_node_pointer_ ? spin_node_pointer_->m_fSpinAccValue : 0.2f;
    // float fDnSlope = spin_node_pointer_ ? spin_node_pointer_->m_fSpinDccValue : 0.2f; //-0.2f

    max_velocity = __max_(max_velocity, 0.02f);
    max_velocity = __min_(max_velocity, 4.0f);    // 1.0

    up_slope_acc = __max_(up_slope_acc, 0.01f);
    up_slope_acc = __min_(up_slope_acc, 1.0f);  // 0.8

    down_slope_acc = __max_(down_slope_acc, 0.01f);
    down_slope_acc = __min_(down_slope_acc, 1.0f);
    down_slope_acc = -down_slope_acc;

    // 低速时减速度:   0.02rad/s^2
    spin_params.small_slope_ = __max_(spin_params.small_slope_, 0.005f);
    spin_params.small_slope_ = __min_(spin_params.small_slope_, 1.0f);
    // 低速巡航速度:0.03rad/s
    spin_params.search_0_value_ = __max_(spin_params.search_0_value_, 0.005f);
    spin_params.search_0_value_ = __min_(spin_params.search_0_value_, 1.0f);

    float transition_value = 0.00f;                  // 0.08 only here not 0
    float sure_area = check_mark_area * 2.0f;
    float small_slope_acc = spin_params.small_slope_; // 2.8degree/s^2
    float search_0_value = spin_params.search_0_value_;
    

    DEBUG_OUT("spin:max_velocity:" << max_velocity);
                                  
    // 增加自旋开放段，缓慢旋转停车 change by yxj at2023-5-16
    Create(max_velocity, max_velocity,
           up_slope_acc, down_slope_acc, small_slope_acc, small_slope_acc,
           transition_value, sure_area, search_0_value, NO_GUIDANCE, check_mark);
}

/**
 * @brief 创建轨迹对象
 *
 * 该函数用于创建轨迹对象，并初始化其相关参数。
 *
 * @param max_value 最大速度
 * @param refrain_value 禁止速度
 * @param up_slope 加速斜率
 * @param down_slope 减速斜率
 * @param up_small_slope 小幅度加速斜率
 * @param down_small_slope 小幅度减速斜率
 * @param transition_value 过渡速度
 * @param sure_area 确定区域
 * @param search_0_value 搜索速度
 * @param guide_type 引导类型
 * @param check_mark 检查标记
 * @param max_dead_distance 最大盲区距离
 */
void TrajNv::Create(float max_value, float refrain_value, float up_slope, float down_slope,
                    float up_small_slope, float down_small_slope, float transition_value, float sure_area,
                    float search_0_value, short guide_type, int check_mark, float max_dead_distance)
{

    if (!traj_pointer_) {
        DEBUG_ERROR_OUT("errro:TrajNv::Create,traj_pointer_ is null");
        return;
    }

    DEBUG_OUT("creat nv traj type:" << traj_pointer_->type_
                                    << ",maxVel:" << max_value << ",RefrainValue:"
                                    << refrain_value << ",acc:" << up_slope << ",dcc:" << down_slope
                                    << ",smallUpAcc:" << up_small_slope << ",smallDownAcc:" << down_small_slope
                                    << ",trVel:" << transition_value << ",attach:" << sure_area
                                    << ",markVel:" << search_0_value << ",gType:" << guide_type
                                    << ",mark:" << check_mark << ",blindDist:" << max_dead_distance);

    max_value_ = max_value;
    refrain_value_ = refrain_value_;
    up_slope_ = up_slope;
    down_slope_ = down_slope;
    up_small_slope_ = up_small_slope;
    down_small_slope_ = down_small_slope;
    transition_value_ = transition_value;
    cycle_ = GetCtrlCycle();
    finished_ = FALSE;
    base_guide_type_ = guide_type;
    rate_ = 0;
    real_progress_ = 0;
    area_ = traj_pointer_ ? traj_pointer_->GetRange() : 0.0f;
    velocity_.SetZero(); // velocity_.fLinear = velocity_.angular_ = 0;
    brake_stage_ = BRAKE_UNLOCK;
    brake_count_ = 0;
    override_end_velocity_ = FALSE;
    desired_spin_progress_ = 0;
    is_steer_ok_ = false;

    // set basnav sureArea adn searchVel when compile nav obj
    sure_area_ = sure_area;
    search_0_value_ = search_0_value;
    guide_type_ = guide_type;
    max_dead_distance_ = max_dead_distance;
    HelperCreate(check_mark);
    ignore_y_error_ = false;
}


/**
 * @brief 开始轨迹导航
 *
 * 开始一个新的轨迹导航任务，设置起始速度、结束速度、工作模式、初始曲率等参数。
 *
 * @param start_value 起始速度
 * @param end_value 结束速度
 * @param work_mode 工作模式
 * @param init_curvature 初始曲率
 * @param init_curvature_known 初始曲率是否已知
 * @param last_traj_type 上一个轨迹类型
 * @param last_steer_angle 上一个转向角度
 */
void TrajNv::Start(float start_value, float end_value, short work_mode,
                    float init_curvature, bool init_curvature_known, short last_traj_type, Angle last_steer_angle)
{

    DEBUG_OUT("start a new run: startSpeed =" << start_value
                                              << ",endSpeed=" << end_value << ",mode=" << work_mode
                                              << ",initCurvature=" << init_curvature
                                              << ",initCurvatureKnown=" << init_curvature_known
                                              << ",lastTrajType=" << last_traj_type);

    if (!VerifyTrajObj()) {
        DEBUG_ERROR_OUT("error: BasNv::Start traj_pointer_ is null");
        return;
    }
    // 1 Start setting progress in advance to 0 0
    velocity_.SetZero(); // add at 20230717
    velocity_.linear_ = start_value;
   
    // 派生实现，切换导航方式
    PreStart(start_value, end_value, work_mode);

    // 2  Planning creation and loading
    // fStartValue = traj_pointer_->GetProfileValue(fStartValue);
    // fEndValue = traj_pointer_->GetProfileValue(fEndValue);
    short traj_type = traj_pointer_->GetType();
    float range = traj_pointer_->GetRange();

    profiler_.Create(max_value_, refrain_value_, /*change*/ up_slope_, down_slope_, up_small_slope_, down_small_slope_,
                      transition_value_, cycle_, traj_type, sure_area_, search_0_value_);

    profiler_.Load(start_value, end_value, range); // change
    // 3 set work mode and clear whell protect stage and count
    work_mode_ = work_mode; // NV_INPUT_SUSPEND
    brake_stage_ = BRAKE_UNLOCK;
    brake_count_ = 0;
    is_steering_ = false;

    // 4 check new curvature and last curvature is smooth
    // 舵角是否到位
    CheckCurvatureSmooth(traj_pointer_, last_traj_type, init_curvature_known, init_curvature, last_steer_angle);

    // 5 原地打舵准备生成新的ID
    SteerDirCmdTrig();

    // 6 stat-do nothing
    OnStart();

    DEBUG_OUT("basnv:Start=" << start_value << ", End=" << end_value
                             << ", Max=" << max_value_ << ", Up=" << up_slope_ << ", Down="
                             << down_slope_);

    DEBUG_OUT("   upSmallAcc=" << up_small_slope_ << ",downSmallAcc=" << down_small_slope_ << ", Tran=" << transition_value_
                               << ", Sure=" << sure_area_ << ", Search=" << search_0_value_
                               << ", FormOK=" << (int)is_steer_ok_);
}

/**
 * @brief 创建轨迹助手对象
 *
 * 根据传入的检查标记创建轨迹助手对象。
 *
 * @param check_mark 检查标记，用于决定是否创建轨迹助手对象。
 *                   如果为非零值，表示需要创建轨迹助手对象；否则不创建。
 */
void TrajNv::HelperCreate(int check_mark)
{
    // NOTE : LASER NO GUIDANCE SPIN NEED TO EDIT HEAR
    bool is_check_mark = check_mark != 0;
    is_check_mark ? check_mark-- : check_mark; // ? why
    guide_helper_.Create(is_check_mark);
}

/**
 * @brief 启动辅助轨迹生成器
 * Set the initial start blind walking distance, etc
 * 在启动轨迹生成器时调用此函数，初始化辅助生成器并设置最大死区距离。
 */ 
void TrajNv::HelperOnStart()
{
    guide_helper_.OnStart(max_dead_distance_);
}

/**
 * @brief TrajNv类中的OnStart函数
 *
 * 该函数是TrajNv类的成员函数，用于在对象启动时执行特定的初始化操作。
 * Called when the navigation is started
 * @details 该函数首先调用HelperOnStart函数，然后调用CheckSpinPathLost函数，并传入参数true。
 *          HelperOnStart函数通常用于执行一些辅助的启动准备工作，而CheckSpinPathLost函数则用于检查路径是否丢失。
 */
void TrajNv::OnStart()
{
    HelperOnStart();
    CheckSpinPathLost(true);
}

/**
 * @brief 预收集轨迹信息
 * Called before calling the function "CollectInfo".
 * 根据引导类型收集轨迹信息，并根据不同的引导类型进行不同的处理。
 */
void TrajNv::PreCollectInfo()
{
    switch (guide_type_) {
        case NO_GUIDANCE:
        {
            guide_helper_.SetTraj(GetTraj(), 0.0f);
            guide_helper_.CollectInfo(static_posture_, true); // FIXME: spin need change here
        }
        break;
        case QR_CODE_GUIDANCE:
        case LASER_GUIDANCE:
        case MAG_GUIDANCE:
        case REFLECTOR_GUIDANCE:
        {
            guide_helper_.SetTraj(GetTraj(), area_);
            guide_helper_.CollectInfo(TrajNv::static_posture_);
        }
        break;
    }
}

/**
 * @brief 检查是否丢失了旋转路径
 *
 * 检查是否发生了路径丢失的情况，特别是在旋转路径中。
 *
 * @param reset 是否重置累计角度
 *
 * @return 发生路径丢失返回true，否则返回false
 */
bool TrajNv::CheckSpinPathLost(bool reset)
{
    bool occur = false;
    bool spin = GetTrajType() == (short)TrajType::SPIN_TRAJ;
    if (!spin) {
        return occur;
    }

    // 最大定位
    static float sum_theta = 0.0f;
    static float last_yaw = 0.0f;
    static float target_theta = PI;
    Velocity vel;
    float yaw = 0.0f;
    bool is_odom_ok = GetOdomDataHuman(vel, yaw);
    if (reset) {
        sum_theta = 0;
        last_yaw = normalizationRad(yaw);
        target_theta = GetTraj()->GetRange();
        DEBUG_OUT("CheckSpinPathLost:reset once;" << "get odomOk=" << is_odom_ok
                                                  << ",vx=" << vel.vel_x_
                                                  << ",vy=" << vel.vel_y_
                                                  << ",w=" << vel.angular_
                                                  << ",yaw=" << yaw << ",lastYaw=" << last_yaw
                                                  << ",sumTheta=" << sum_theta
                                                  << ",targetTheta=" << target_theta);
        return occur;
    }

    if (!is_odom_ok) {
        yaw = last_yaw;
        DEBUG_ERROR_OUT("odom time out force set yaw=" << yaw << ",lastYaw=" << last_yaw);
    }

    float normal_yaw = normalizationRad(yaw);
    sum_theta += getRadDistance(last_yaw, normal_yaw);
    DEBUG_OUT("odom yaw=" << yaw
                          << ",normalYaw=" << normal_yaw
                          << ",lastYaw=" << last_yaw
                          << ",sumTheta=" << sum_theta
                          << ",odomOk=" << is_odom_ok);

    last_yaw = normal_yaw;

    if (fabs(sum_theta) > (target_theta + TO_RADIAN(MAX_DEAD_SPIN_ANGLE_DEGREE))) {
        occur = true;
        DEBUG_ERROR_OUT("spin angle overlimit;sumTheta=" << sum_theta << ",targetTheta=" << target_theta
                                                         << ",limitAngle=" << MAX_DEAD_SPIN_ANGLE_DEGREE);

        TryToOccurPathLost("spin angle overlimit 90 degree ", true);
    }

    return occur;
}

/**
 * @brief NoGuideReloadArea 函数用于处理无引导情况下的重载区域更新。
 * no guide spin but use laser data(angle) to check target
 * 该函数主要用于检查当前轨迹是否为自旋轨迹，并根据实际情况更新重载区域。
 */
void TrajNv::NoGuideReloadArea()
{
    DEBUG_OUT("NoGuideReloadArea:get pose start");
    // 是否是自旋且有定位数据
    float x = 0, y = 0, t = 0, q = 0;
    bool ret = GetLaserAutoPos(x, y, t, q, false, true);
    bool spin = GetTrajType() == (short)TrajType::SPIN_TRAJ;
    if (!spin) {
        DEBUG_OUT("NoGuideReloadArea: is not spin normal  not valid");
        return;
    }

    // 检测是否自旋超差
    CheckSpinPathLost(false);

    // 定位数据是否有效
    if (!ret) {
        DEBUG_ERROR_OUT("error: spin  traj:get pose failed");
        return;
    }

    // 实际的旋转角度
    Angle current_angle(t, AngleMode::InRadian);
    SpinTrajectory *spain_traj = dynamic_cast<SpinTrajectory *>(GetTraj());
    if (!spain_traj) {
        DEBUG_ERROR_OUT("error: spin  traj is nullptr");
        return;
    }

    // 顺逆时针判断,判断角度差，real_remain_angle:实际剩余距离
    bool is_clockwise = spain_traj->GetTurnDir() == TurnDirTag::Clockwise;
    Angle angle1 = current_angle - spain_traj->EndHeading();
    Angle real_remain_angle = is_clockwise ? angle1 : -angle1;    // angle2 = GetTraj()->EndHeading() - current_angle;
    Angle theory_angle = spain_traj->HeadingFun();

    // 理论规划剩余距离
    float theory_remain_angle = profiler_.GetTheoryArea();        // read spin
    float real_remain_rad = real_remain_angle.NormAngle2();       // [-PI,PI]
    float target_threshold = TO_RADIAN(0.05);                     // 0.25dgree

    // FIXME:problem
    // 前150度，不做处理
    if (theory_remain_angle >= TO_RADIAN(150)) {
        if (real_remain_rad > 0) {
            profiler_.ReloadArea(real_remain_rad); // write
            DEBUG_OUT("heoryRemainAngle > 150 degree:real_remain_rad > 0");
        } else {
            profiler_.ReloadArea(theory_remain_angle); // write
            DEBUG_OUT("heoryRemainAngle > 150 degree:real_remain_rad < 0");
        }

        DEBUG_OUT("theory_remain_angle > 150degree: reload theory angle; when current_angle=" << current_angle.radian_
                                                                                       << ",endAngle:" << spain_traj->EndHeading().radian_
                                                                                       << ",remainAngle=" << real_remain_angle.radian_
                                                                                       << ",real_remain_rad=" << real_remain_rad
                                                                                       << ",theory_remain_angle=" << theory_remain_angle);

        return;
    }
    // 角度剩余
    if (real_remain_rad <= target_threshold && theory_remain_angle <= PI_4) { // degree  (+)positive degree to (-)negative
        profiler_.ReloadArea(0.0f); // write: spin
        OnFinish();                  // add by qf
        DEBUG_OUT("spinTraj set ok finish spin; when current_angle=" << current_angle.radian_
                                                                << ",endAngle:" << spain_traj->EndHeading().radian_
                                                                << ",remainAngle=" << real_remain_angle.radian_
                                                                << ",real_remain_rad=" << real_remain_rad
                                                                << ",theory_remain_angle=" << theory_remain_angle);
        return;
    }

    profiler_.ReloadArea(real_remain_rad); // write
    DEBUG_OUT("spin: realod real remainRad=" << real_remain_rad << ",theory_remain_angle=" << theory_remain_angle << ",trajTheoryAngle=" << theory_angle.radian_);
}

/**
 * @brief 重新加载引导区域的剩余距离
 *
 * 重新计算引导区域的剩余距离并更新。
 *
 * 注意：该函数包含待修复的问题（FIXME），需要补偿实时进度。
 */
void TrajNv::GuideReloadArea()
{
    // FIXME:补偿实时进度
    float real_progress;
    bool ret = guide_helper_.GetMeasuredRelProgress(real_progress);
    BOOL move_forward = GetTraj()->WheelMoveDir() == MoveDirTag::Forward;

    if (ret) {
        // laser feedback ok to calc remain area
        float remain_area = GetTraj()->GetRange() - real_progress;
        bool is_stop = fabs(profiler_.GetEndValue()) <= 0.0001f;
        // 刹车距离补偿功能
        if (is_stop) {
            float stop_distance = move_forward ? MotionParm::forward_stop_dis_ : MotionParm::backward_stop_dis_;
            remain_area -= stop_distance;
            DEBUG_OUT("#5 laser adjust:"
                      << ",fStopDis : " << stop_distance << ",adjust later reamain area=" << remain_area);
        }
        float old_area = profiler_.GetCurArea(); // read ;use for record log
        profiler_.ReloadArea(remain_area);       // write
        DEBUG_OUT("reload real RemainArea=" << remain_area << ",oldCurArea=" << old_area);

        if (GetTrajType() == (short)TrajType::BEZIER_TRAJ) {
            BezierTrajectory *bezier_traj = (BezierTrajectory *)GetTraj();
            if (bezier_traj) {
                // change by yxj
                bezier_traj->SetReloadAreaFlag(true);
            }
        }
    }
}

/**
 * @brief 预生成速度
 * For various navigation modes, judge landmark signs in the open section,
 * report normal completion or load landmark width
 * 根据引导类型预生成速度。
 */
void TrajNv::PreGenerateVelocity()
{
    switch (guide_type_) {
        case NO_GUIDANCE:
            NoGuideReloadArea();
            break;
        case QR_CODE_GUIDANCE:
        case LASER_GUIDANCE:
        case MAG_GUIDANCE:
        case REFLECTOR_GUIDANCE:
            GuideReloadArea();
            break;
    }
}

/**
 * @brief 生成激光导航中的角速度
 *
 * 根据当前的运动状态生成适当的角速度调整值，用于激光导航过程中的路径调整。
 *
 * @return 无返回值
 */
void TrajNv::LaserGenAngularVel()
{
    if (guide_type_ == NO_GUIDANCE)
        return;

    bool feedback_ok = FALSE;
    ModifyCtrl modify_ctrl;
    float linear_velocity = GetLinearVel();
    float angular_velocity = GetAngularVel();
    short traj_type = GetTrajType();

    guide_helper_.SetLinearVel(linear_velocity);
    guide_helper_.SetAngularVel(angular_velocity);
    modify_ctrl = guide_helper_.GenAngularVel(static_posture_, feedback_ok, ignore_y_error_);

    if (!feedback_ok) {
        DEBUG_ERROR_OUT("error:location pos is failed;can not modify vel or w");
        return;
    }

    // 最大纠偏分量上限值0.1m/s
    const float max_delta_vx = 0.1f, max_delta_vy = 0.1f;
    float radius = MotionParm::min_adjust_radius_;
    radius = radius <= 0.00001f ? 1.0f : radius;
    float low_divide = traj_type == (short)TrajType::LINE_TRAJ ? 3 : 2;
    float hight_divide = traj_type == (short)TrajType::LINE_TRAJ ? 1.5 : 1;
    radius = fabs(linear_velocity) < 0.5f ? (radius / low_divide) : (radius / hight_divide);

    // 根据当前的规划速度 限制当前纠偏量
    LimitModifyVelocity(velocity_, modify_ctrl);

    // 如果此时线速度为0，角速度纠偏则也为0(防止原地转圈纠偏)
    float max_delat_w = fabs(linear_velocity / radius);
    modify_ctrl.angular_velocity = Limit(modify_ctrl.angular_velocity, max_delat_w);
    modify_ctrl.delta_velocity_x = Limit(modify_ctrl.delta_velocity_x, max_delta_vx);
    modify_ctrl.delta_velocity_y = Limit(modify_ctrl.delta_velocity_y, max_delta_vy);

    DEBUG_OUT("out modify_ctrl:w=" << modify_ctrl.angular_velocity << ",vx=" << modify_ctrl.delta_velocity_x
                                  << ",vy=" << modify_ctrl.delta_velocity_y
                                  << ",maxW=" << max_delat_w
                                  << ",maxVx=" << max_delta_vx
                                  << ",maxVy=" << max_delta_vy
                                  << ",maxR=" << radius);
    velocity_.angular_ += modify_ctrl.angular_velocity;
    velocity_.vel_x_ += modify_ctrl.delta_velocity_x;
    velocity_.vel_y_ += modify_ctrl.delta_velocity_y;
}

/**
 * @brief 修改速度，同时限制y方向和x方向的纠偏量
 *
 * 此函数用于修改速度，并在修改过程中对y方向和x方向的纠偏量进行限制。
 * 全方位车型纠偏限制：限制x y 方向的速度(防止舵角打舵过大)
 * @param planned_velocity 计划速度，类型为Velocity
 * @param modify 修改控制信息，类型为ModifyCtrl
 */
void TrajNv::LimitModifyVelocity(const Velocity &planned_velocity, ModifyCtrl &modify)
{
    // #1 限制y方向纠偏量
    const float max_steer_angle = TO_RADIAN(15); // 20度
    float delta_vy = modify.delta_velocity_y;

    float angle_y = 0.0f;
    if (fabs(planned_velocity.vel_x_) > 0.001f) {
        angle_y = atan(delta_vy / planned_velocity.vel_x_); //[-pi/2,pi/2]
    }

    angle_y = Limit(angle_y, max_steer_angle); // 限制纠偏角度最大20度[-20,20]
    // angle_y = planned_velocity.vel_x_ > 0 ? angle_y : -angle_y; // 添加符号反转
    modify.delta_velocity_y = tan(angle_y) * planned_velocity.vel_x_;

    // #2 限制x方向纠偏量
    float delta_vx = modify.delta_velocity_x;
    float angle_x = 0.0;
    if (fabs(planned_velocity.vel_y_) > 0.001f) {
        angle_x = atan(delta_vx / planned_velocity.vel_y_); //[-pi/2,pi/2]
    }

    angle_x = Limit(angle_x, max_steer_angle); // 限制纠偏角度最大20度[-20,20]
    // angle_x = planned_velocity.vel_y_ > 0 ? angle_x : -angle_x; // 添加符号反转
    modify.delta_velocity_x = tan(angle_x) * planned_velocity.vel_y_;

    DEBUG_OUT("LimitModifyV:delta_vy(old)=" << delta_vy << ",planned_velocity.vel_x_=" << planned_velocity.vel_x_
                                           << ",angle_y=" << angle_y << ",delta_vy(new)=" << modify.delta_velocity_y
                                           << ",delta_vx(old)=" << delta_vx << ",planned_velocity.vel_y_=" << planned_velocity.vel_y_
                                           << ",angle_x=" << angle_x << ",delta_vx(new)=" << modify.delta_velocity_x);
}

/**
 * @brief 生成轨迹的角速度引导
 *
 * 根据不同的引导类型生成相应的角速度修正值。
 */
void TrajNv::GuideGenAngularVel()
{
    // Generate deflection correction angular velocity
    switch (guide_type_)
    {
    case NO_GUIDANCE:
        // NoGuideGenAngularVel();
        break;
    case QR_CODE_GUIDANCE:
    case LASER_GUIDANCE:
    case MAG_GUIDANCE:
    case REFLECTOR_GUIDANCE:
        LaserGenAngularVel();
        break;
    }
}

/**
 * @brief 生成速度后处理
 *
 * 在生成速度后调用此方法，用于生成角速度。
 */
void TrajNv::AfterGenerateVelocity()
{
    GuideGenAngularVel();
}

/**
 * @brief 获取导航级别
 *
 * 根据当前导航辅助器的定位反馈结果，返回相应的导航级别。
 *
 * @return 返回导航级别，具体为：
 *         - NAV_LEVEL_DEAD_RECKON: 无导航的步行方式
 *         - NAV_LEVEL_NORMAL: 正常导航级别
 *         - NAV_LEVEL_DEAD_RECKON: 推算导航级别
 *         - NAV_LEVEL_FAIL: 导航失败级别
 */
TagNavLevel TrajNv::GetNavLevel()
{
    TagNavLevel level = NAV_LEVEL_DEAD_RECKON; // Navigation type walking without navigation

    if (NO_GUIDANCE == guide_type_) {
        return level;
    }

    switch (guide_helper_.GetLocFeedBack()) {
        case LocationFeedback::LOC_FEEDBACK_OK:
        case LocationFeedback::LOC_FEEDBACK_WAITING:
            level = NAV_LEVEL_NORMAL;
            break;
        case LocationFeedback::LOC_DEAD_RECKONING:
            level = NAV_LEVEL_DEAD_RECKON;
            break;
        default:
            level = NAV_LEVEL_FAIL;
            break;
    }

    return level;
}

/**
 * @brief 获取导航偏差
 *
 * 获取当前导航的偏差值，包括横向偏差和航向偏差。
 *
 * @param x      横向偏差（单位：米）
 * @param theta  航向偏差（单位：弧度）
 */
void TrajNv::GetNavDeviation(float& dev_x, float& dev_y, float& dev_t)
{
    if (NO_GUIDANCE == guide_type_) {
        dev_x = dev_y = dev_t = 0.0f;
        return;
    }
    dev_x = guide_helper_.GetProjectData().deviation_progress_;
    dev_y = guide_helper_.GetProjectData().x_;
    dev_t = guide_helper_.GetProjectData().theta_;
}

/**
 * @brief 更新车辆状态
 *
 * 此函数用于更新车辆的状态信息，包括车辆的位置、速度、方向、导航偏差等信息。
 *
 * 更新过程包括：
 * 1. 计算车辆的绝对进度。
 * 2. 获取车辆的导航偏差。
 * 3. 更新车辆的状态结构体（VhcStatus）中的各项参数。
 * 4. 实时更新进度信息。
 * 5. 输出调试信息。
 */
void TrajNv::UpdateVehicleStatus()
{
    float from_progress = GetTraj()->GetRange() + GetTraj()->from_;
    from_progress = fabs(from_progress) < 0.00001f ? 0.00001 : from_progress; // if Divided by 0

    GetNavDeviation(VhcStatus.deviation_progress_, VhcStatus.dev_x_, VhcStatus.dev_theta_);
    VhcStatus.traj_type_ = GetTrajType();
    VhcStatus.abs_progress_ = GetRelProgress() + GetTraj()->GetFrom();
    VhcStatus.cur_posture_ = GetPosture();
    VhcStatus.steer_angle_ = GetSteerAngle();
    VhcStatus.velocity_.linear_ = GetLinearVel();
    VhcStatus.velocity_.angular_ = GetAngularVel();
    VhcStatus.prog_percent_ = fabs(VhcStatus.abs_progress_ / from_progress);
    VhcStatus.nav_level_ = GetNavLevel();
    VhcStatus.task_status_ = TaskStatus::Run;
    VhcStatus.mode_ = profiler_.GetMode(); // read
    VhcStatus.quality_ = guide_helper_.GetQuality();
    VhcStatus.wheel_move_dir_ = GetMoveDir().move_dir_tag_;

    // 实时更新进度信息
    SingleCheckObj->UpdateProgRunData(VhcStatus.from_node_, VhcStatus.to_node_,
                                      VhcStatus.abs_progress_, VhcStatus.prog_percent_);

    DEBUG_OUT("fromNode=" << VhcStatus.from_node_
                          << ",toNode=" << VhcStatus.to_node_
                          << ",Progress=" << VhcStatus.abs_progress_ << " m "
                          << ",ProgPercent=" << VhcStatus.prog_percent_
                          << "pst.x y t: "
                          << VhcStatus.cur_posture_.x_ << ", "
                          << VhcStatus.cur_posture_.y_ << ", "
                          << VhcStatus.cur_posture_.theta_
                          << ",fDevProg:"<<VhcStatus.deviation_progress_
                          << ",fDevX:" << VhcStatus.dev_x_
                          << ",fDevThita:" << VhcStatus.dev_theta_
                          << ",angSteer(degree)=" << VhcStatus.steer_angle_.Degree());
}


/**
 * @brief TrajNv类的CycleRoutine成员函数
 *
 * 在每个周期执行一系列操作，包括打舵逻辑复位、定位数据有效性检测、速度规划和车辆状态更新等。
 *
 * @note 此函数在每个周期被调用，用于更新车辆状态和生成新的速度规划。
 */
void TrajNv::CycleRoutine()
{
    VhcStatus.loc_type_ = TagStateType::DYNAMIC_LOCATION;
    // FIXME: this cycleRoutine, m_nLocType may be set to STATIC_LOCATION
    // 每个周期复位打舵逻辑
    is_steering_ = false;

    // 地标和导航辅助对象 检测定位数据的有效性
    PreCollectInfo();

    // 暂时未做逻辑处理
    CollectInfo();

    // 打舵是否到位
    if (!SteerReadyOk()) {
        return;
    }

    // 设置停车距离
    PreGenerateVelocity();

    // 产生规划速度处处和速度规划模式切换和恢复(轮保护)
    GenerateVelocity();

    // 产生纠偏叠加速度
    AfterGenerateVelocity();

    // 设置速度输出 SetAutoModeNavVel
    Drive();
   
    UpdateVehicleStatus();
    PathVerify();
}

/**
 * @brief 路径验证函数
 *
 * 该函数用于验证路径的有效性，包括缺失触发和距离超限触发两种情况。
 *
 * @details
 * 1. 如果检测到缺失触发（MissTrigger），则尝试触发路径丢失事件，并输出调试错误信息。
 * 2. 如果检测到距离超限触发（DisOverLimitTrigger），则尝试触发路径丢失事件，并输出调试错误信息。
 */
void TrajNv::PathVerify()
{
    std::string miss_id, dis_msg;
    if (SingleCheckObj->MarkMissTrigger(miss_id)) {
        TryToOccurPathLost("qrCode miss:id: " + miss_id, true);
        DEBUG_ERROR_OUT("MissTrigger: TryToOccurPathLost:id=" << miss_id);
    }

    if (SingleCheckObj->DisOverLimitTrigger(dis_msg)) {
        TryToOccurPathLost("path distance error:" + dis_msg, true);
        DEBUG_ERROR_OUT("DisOverLimitTrigger: TryToOccurPathLost:" << dis_msg);
    }
}

/**
 * @brief 获取轨迹类型
 *
 * 获取当前轨迹对象的类型。如果轨迹对象不为空，则返回轨迹对象的类型；否则返回未定义轨迹类型。
 *
 * @return 返回轨迹类型，如果轨迹对象为空则返回 UNDEFINED_TRAJ
 */
short TrajNv::GetTrajType()
{
    if (GetTraj() != nullptr) {
        return GetTraj()->GetType();
    } else {
        return (short)TrajType::UNDEFINED_TRAJ;
    }
}

/**
 * @brief 判断转向是否准备好
 *
 * 检查转向是否准备好，并根据当前状态返回相应的布尔值。
 *
 * @return 如果转向已准备好，则返回 true；否则返回 false。
 */
bool TrajNv::SteerReadyOk()
{
    if (!traj_pointer_) {
        DEBUG_ERROR_OUT("error: TransFormOk;traj_pointer_ is null");
        is_steer_ok_ = false;
        return is_steer_ok_;
    }

    if (!is_steer_ok_) {
        // 原地打舵过程是否需要紧急停止?
        bool shift_pattern = traj_pointer_->GetDrivePattern() == (short)TagTrajDrivePattern::DRIVE_PATTERN_SHIFT;
        if (shift_pattern && !IsOmniDirection()) {
            work_mode_ = NV_SUSPEND;
            velocity_.SetZero();
            DEBUG_ERROR_OUT("Error: shift can not do because agv is not OmniDirectional");
            return false;
        }

        if (!HasSteer()) {
            is_steer_ok_ = true;
            return is_steer_ok_;
        }

        traj_pointer_->SetRelProgress(0, 0);
        if (Steering(traj_pointer_)) {
            is_steer_ok_ = TRUE;
            DEBUG_WARN_OUT("OK;SteerReadyOk");
        }
    }

    return is_steer_ok_;
}

//   According to whether there is an event,
//   switch wheel protection mode/limit planning speed,
//   set planning mode + track, generate speed vector, rudder angle, etc
//   Generate drive velocity.
/**
 * @brief 生成速度函数
 *
 * 此函数根据当前的工作模式和外部控制信号生成相应的速度值。
 *
 * @details
 * 1. 首先检查轨迹对象是否有效，如果无效则直接返回。
 * 2. 获取当前的控制模式，判断是否有紧急停止、正常停止、暂停等信号。
 * 3. 根据当前工作模式（正常运行或挂起）设置速度配置文件的工作模式。
 *   - 在正常运行模式下，如果有紧急停止、缓慢停止或打舵信号，则切换到挂起模式。
 *   - 在挂起模式下，如果已经停止且可以恢复或正在打舵，则检查舵角是否到位，到位后进入正常运行模式。
 * 4. 调用ProfilerOutVel函数生成当前速度值。
 * 5. 检查速度配置文件是否完成，如果完成则调用OnFinish函数并输出计划完成的信息。
 * 6. 调用GenerateProgVelSteerAngle函数生成进度速度和转向角度。
 */
void TrajNv::GenerateVelocity()
{
    if (!VerifyTrajObj()) {
        return;
    }

    // #1 get cur served id
    uint8_t control_mode = EventOccur();
    bool emg_stop = control_mode == agv_msgs::msg::AGVEvent::ESTOP || OtherStopSignal() == static_cast<int>(DoSteerState::SteerError);
    bool normal_stop = control_mode == agv_msgs::msg::AGVEvent::STOP || OtherSlowStopSignal();
    bool pause_occur = ReadyPauseTask();
    short traj_type = traj_pointer_->GetType();
    bool steering = OtherStopSignal() == static_cast<int>(DoSteerState::Steering);
    bool steer_brake_lock = OtherStopSignal() == static_cast<int>(DoSteerState::SteerBrakeLock); // 抱闸锁住 或使能未上
    // 抑制速度降低
    bool dec_speed = ProfilerRefrain(traj_type);

    if (nullptr == traj_pointer_) {
        DEBUG_OUT("Error: traj_pointer_ = NULL");
        return;
    }

    if (finished_) {
        DEBUG_OUT("GenerateVelocity: finished");
        return;
    }

    // #2 mode change: NV_NORMAL  <-->  NV_SUSPEND
    //  set profile mode
    switch (work_mode_) {
        case NV_NORMAL: // 正常运行模式下
        {
            // 区分紧急停止和缓慢停止
            bool force_stop = emg_stop || steering || steer_brake_lock;
            bool slow_stop = normal_stop || pause_occur;
            if (force_stop || slow_stop) {
                work_mode_ = NV_SUSPEND;                                                   // #1 正常运行时，触发急停跳转到挂起模式
                profiler_.SetMode(force_stop ? PROFILE_MODE_ESTOP : PROFILE_MODE_SUSPEND); // use
                SteerDirCmdTrig();                                                         // 缓停挂起时，触发设置新的打舵ID
            } else {
                // 正常运行,是否需要限制速度
                profiler_.SetMode(dec_speed ? PROFILE_MODE_REFRAIN : PROFILE_MODE_NORMAL); // use
            }
        }
        break;
        case NV_SUSPEND: // 挂起模式下
        {
            // 是否能够恢复
            bool can_recover = (!emg_stop && !normal_stop && !pause_occur);
            // 是否已经停车了
            bool already_stop = IsStop(traj_type);
            // 能够恢复或者正在打舵
            if (already_stop && (can_recover || steering)) {
                // 判断舵角是否到位(内部包含chassis_not_ready的判断)
                if (Steering(traj_pointer_)) {
                    // 舵角到位完成 进入NV_NORMAL运行模式
                    work_mode_ = NV_NORMAL;
                    profiler_.SetMode(dec_speed ? PROFILE_MODE_REFRAIN : PROFILE_MODE_NORMAL); // use
                }
            } else {
                //  等待恢复
                //  处于挂起模式(急停或缓停逻辑)
                profiler_.SetMode(emg_stop ? PROFILE_MODE_ESTOP : PROFILE_MODE_SUSPEND);       // use
            }
        }
        break;
    }

    // #3 (temp do nothing) When the navigation deviation is too large or the distance is detected, slow down
    // ProfilerRefrain(traj_type);

    // #4 profiler generate  to curValue by set max speed
    // 调用一次 计算一次内部当前m_fCurValue(m_profiler 使用)
    // 为GenerateProgVelSteerAngle 内部数据做准备
    float current_value = 0.0f;
    ProfilerOutVel(current_value);

    // #6 profiler is done
    //  The planning is completed and the path ends
    //(for example, the landmark width is loaded before,
    //  and when the landmark width is completed, the report is completed)
    if (profiler_.Done()) { // use
        OnFinish();
        DEBUG_OUT("plan on finish");
    }

    GenerateProgVelSteerAngle();
}

/**
 * @brief TrajNv类的Drive函数
 *
 * 根据当前的行驶模式，调用相应的行驶方法，并设置自动模式下的导航速度。
 *
 * 如果当前行驶模式是转弯模式，则调用DriveTurn函数；否则调用DriveShift函数。
 *
 * 如果当前不是转向状态，则设置自动模式下的导航速度，并根据转向方向更新禁止反向标识。
 *
 * 如果当前是转向状态，则输出规划速度信息。
 */
void TrajNv::Drive()
{
    // 1 Turn Model
    bool turn_pattern = traj_pointer_->GetDrivePattern() == (short)TagTrajDrivePattern::DRIVE_PATTERN_TURN;
    if (turn_pattern) {
        // for diff driver; only use turn model
        DriveTurn();
    } else { // 2:shift Model
        DriveShift();
    }

    // 输出控制速度
    if (!is_steering_) {
        SetAutoModeNavVel(velocity_.vel_x_, velocity_.vel_y_, velocity_.angular_);
        // 当运行时，侧移等需要强制限制舵角正反向时，需要更新设置禁止反向标识
        // 否则禁止反向标识，只有在原地打舵时，才会更新
        // 切换瞬间，底盘程序会出现steering is true问题，暂时不开启
        int steer_dir = (int)SteerDirReport::SteerDirAuto;
        Velocity vel = traj_pointer_->VelocityForSteerAngle(0.0001, steer_dir);
        DoSteerCmd(vel, steer_dir, false);
        DEBUG_OUT("steer is ready;Drive:vx=" << velocity_.vel_x_ << ",vy=" << velocity_.vel_y_ << ",w=" << velocity_.angular_);
    } else {
        //  velocity_.SetZero();
        DEBUG_OUT("steer is not ready;Drive:Planning  vx=" << velocity_.vel_x_ << ",vy=" << velocity_.vel_y_ << ",w=" << velocity_.angular_);
    }
}

/**
 * @brief 计算线性约束速度
 *
 * 根据当前货物状态（是否装载）和路径指针，计算线性约束速度。
 *
 * @return 返回计算得到的线性约束速度，如果路径指针为空则返回0.0f。
 */
float TrajNv::LinearRefrainVel()
{
    if (path_pointer_ != nullptr) {
        float velocity;
        if (use_speed_level_) {
            velocity = 0.1f;
        } else {
            velocity = level_ * 0.001f;
            velocity = __min_(velocity, 0.2f);
        }

        // 货物状态
        bool load = MotionParm::load_flags_.current_load;
        float max_velocity = load ? path_pointer_->velocity_limit_load_ : path_pointer_->velocity_limit_;
        DEBUG_OUT("LinearRefrainVel:" << ",load=" << (int)(load)
                                      << ",max_velocity=" << max_velocity
                                      << ",loadVel=" << path_pointer_->velocity_limit_load_
                                      << ",unloadVel=" << path_pointer_->velocity_limit_);

        // float max_velocity = path_pointer_->velocity_limit_;
        return __min_(max_velocity, velocity);
    } else
        return 0.0f;
}

/**
 * @brief 计算线性巡航速度
 *
 * 根据当前路径指针和加载状态，计算并返回线性巡航速度。
 *
 * @return 返回线性巡航速度，单位为米/秒。如果路径指针为空，则返回0。
 */
float TrajNv::LinearCruiseVel()
{
    if (path_pointer_ != nullptr) {
        // float velocity = 0.0f;
        // if (use_speed_level_)
        //     velocity = CMotionParm::NormalVel(level_);
        // else
        //     velocity = level_ * 0.001f;

        bool load = MotionParm::load_flags_.current_load;
        float max_velocity = load ? path_pointer_->velocity_limit_load_ : path_pointer_->velocity_limit_;

        DEBUG_OUT("LinearCruiseVel:" << "load=" << (int)(load)
                                     << ",max_velocity=" << max_velocity
                                     << ",loadVel=" << path_pointer_->velocity_limit_load_
                                     << ",unloadVel=" << path_pointer_->velocity_limit_);

        return __min_(max_velocity, 2.5f); // change by yxj
    } else {
        return 0.0f;
    }
}

/**
 * @brief 驱动转弯功能
 *
 * 如果当前轨迹的类型为旋转轨迹（SPIN_TRAJ），则根据是否已完成旋转来设置速度。
 *
 * 如果轨迹已完成，则将线速度和角速度都设置为0，并输出调试信息。
 */
void TrajNv::DriveTurn()
{
    if (traj_pointer_->GetType() == (int)TrajType::SPIN_TRAJ) {
        if (finished_) {
            // velocity_.fLinear = 0.0f;
            // velocity_.angular_ = 0.0f;
            velocity_.SetZero();
            DEBUG_OUT("spin traj finish; set vel zero");
        }
    }
}

/**
 * @brief 在轨迹开始前进行预处理
 *
 * 在轨迹开始之前，调用此方法来进行预处理。
 *
 * @param fStartValue 起始值，表示轨迹开始的数值
 * @param fEndValue 结束值，表示轨迹结束的数值
 * @param nWorkMode 工作模式，用于指定轨迹的运行模式
 */
void TrajNv::PreStart(float start_value, float end_value, short work_mode)
{
    traj_pointer_->SetRelProgress(0.0f, 0.0f);

    VhcStatus.from_node_ = start_node_id_;
    VhcStatus.to_node_ = end_node_id_;
    
    // DEBUG_OUT("start update node id:StartNode=" << start_node_ << ",EndNode=" << end_node_);

    StartLaserDetectAreaAndNavMode(path_pointer_, GetTraj());

    if (!GetTraj()) {
        DEBUG_OUT("PreStart: cur traj is null");
        return;
    }

    DEBUG_OUT("Start to (Ready): " << start_node_id_ << " to " << end_node_id_ <<
                                 ": x y angle namely " << GetPosture().x_ << 
                                 " " << GetPosture().y_ << " " << GetPosture().theta_ << 
                                 " ;" << " #trajType=" << GetTrajType());
}

bool TrajNv::StartLaserDetectAreaAndNavMode(Path *path, Trajectory *traj)
{
    // 尝试更新一次纠偏参数
    DEBUG_OUT("StartLaserDetectAreaAndNavMode start");
    MotionParm::AutoUpdateAllParam();

    // 尝试增加新的二维码ID检测
    DEBUG_OUT("try to AddNewLandMark start");
    if (!SingleCheckObj->AddNewPath(path, traj))
        DEBUG_ERROR_OUT("checkMan bAddOk =" << false);
    DEBUG_OUT("try to AddNewLandMark end");

    if (path) {
        // #2 告知定位模块的导航方式
        USHORT guide_type = path->GuideType();
        ChangeNavMode(guide_type);

        //    路径切换激光避障方案(当所有配置方案情况都不满足，才采用此避障方案)
        //    设置路径属性bit位
        std::string scenes = std::to_string(path->GetDetectPlan());
        SetNavPathScenesData(scenes);
        uint16_t bits = path->Bits();
        SetNavPathBitsData(bits);

        // 路径word属性
        int word1 = -1;
        int word2 = -1;
        path->Words(word1, word2);
        SetNavPathWordData(word1, word2);
        ignore_y_error_ = TestBit(bits, IGNORE_Y_ERROR_BITS);
        DEBUG_OUT("normal path:SeNavPathData scenes:" << scenes
                                                      << ",bits=" << bits
                                                      << ",ignoreYError=" << ignore_y_error_);
    } else {
        bool bSpin = traj ? traj->GetType() == (short)TrajType::SPIN_TRAJ : false;
        if (bSpin && spin_node_pointer_) {
            //    路径切换激光避障方案(当所有配置方案情况都不满足，才采用此避障方案)
            //    设置路径属性bit位
            std::string scenes = std::to_string(spin_node_pointer_->GetDetectPlan());
            SetNavPathScenesData(scenes);
            uint16_t bits = spin_node_pointer_->Bits();
            SetNavPathBitsData(bits);
            SetNavPathWordData(-1, -1);
            DEBUG_OUT("spin path:SeNavPathData scenes:" << scenes
                                             << ",bits=" << bits);
        } else {
            DEBUG_ERROR_OUT("error: ChangeLaserDetectArea failed,cur path is null");
        }
    }

    // 告知定位模块的轨迹类型
    if (traj) {
        int type = traj->GetType();
        ChangeTrajType(type);
        DEBUG_OUT("report current traj type ok,nCurTrajType:" << type);
    } else {
        DEBUG_ERROR_OUT("error: report current traj type failed,cur pTraj is null");
    }

    DEBUG_OUT("StartLaserDetectAreaAndNavMode end");
    return true;
}

/**
 * @brief 判断是否可以进行机架跟随
 *
 * 该函数用于判断当前机架是否可以跟随旋转节点进行旋转调整。
 *
 * @return 如果可以进行机架跟随，则返回true；否则返回false。
 */
bool TrajNv::CanRackFollowUp()
{
    Node *spin_node_pointer = GetSpinNodeObj();
    if (!spin_node_pointer) {
        DEBUG_OUT("can not need rack follow up");
        return false;
    }

    bool ret = spin_node_pointer->EnableTableTunning();
    DEBUG_OUT("EnableTableTunning enable=" << ret);
    return ret;
}

/**
 * @brief 检查轨迹的曲率平滑性
 *
 * 检查给定轨迹的曲率平滑性，并根据条件更新舵机状态。
 *
 * @param traj 指向轨迹对象的指针
 * @param last_traj_type 上一个轨迹的类型
 * @param init_curvature_known 初始曲率是否已知
 * @param init_curvature 初始曲率
 * @param last_steer_angle 上一个舵机的角度
 */
void TrajNv::CheckCurvatureSmooth(Trajectory *traj, short last_traj_type, bool init_curvature_known, float init_curvature, Angle last_steer_angle)
{
    if (!traj) {
        DEBUG_ERROR_OUT("error: CheckCurvatureSmooth:traj is null");
        return;
    }

    // short newTrajType = pNewTraj->GetType();
    // float fStartCurvature = pNewTraj->StartCurvature();
    // bool bShiftPattern = pNewTraj->GetDrivePattern() == DRIVE_PATTERN_SHIFT;
    Angle start_angle = traj->StartSteerAngle();

    if (!HasSteer()) {
        is_steer_ok_ = true;
        DEBUG_OUT("start: Is not OmniDirection ;CheckCurvatureSmooth: set is_steer_ok_=" << is_steer_ok_);
        return;
    }

    // 如果两个角度差小于4度 则任务舵到位 m_fRad:[0-2PI] -> [-PI,PI]
    // 且上一次曲率已知
    float diff_angle = fabs(NormAngle2(start_angle.radian_ - last_steer_angle.radian_));
    diff_angle = TO_DEGREE(diff_angle);
    if (init_curvature_known && diff_angle < MAX_STEER_ANGLE_SMOOTH) {
        is_steer_ok_ = true;
    }

    DEBUG_OUT("start: CheckCurvatureSmooth:is_steer_ok_=" << is_steer_ok_ << ",diff_angle=" << diff_angle
                                                        << ",start_angle.m_fRad=" << start_angle.radian_ << ",lastSteerAngle.m_fRad=" << last_steer_angle.radian_);
}

/**
 * @brief 执行转向操作
 *
 * 根据当前轨迹执行转向操作，并返回转向是否成功。
 *
 * @param traj 指向当前轨迹的指针
 *
 * @return 若转向成功则返回 true，否则返回 false
 */
bool TrajNv::Steering(Trajectory *traj)
{
    bool ret = false;
    if (!HasSteer()) {
        ret = true;
        DEBUG_OUT("steering is finish: agv is not omni");
        return ret;
    }

    if (!traj) {
        DEBUG_ERROR_OUT("steering error: cur traj is null");
        return ret;
    }

    // 打舵前延迟
    int delay_ms = MotionParm::steer_ready_params_.delay_ms_;
    if (!SteerCmdDelayFinish(delay_ms)) {
        DEBUG_OUT("SteerCmdDelayFinish wait....delay_ms=" << delay_ms);
        return ret;
    }

    int steer_dir = (int)SteerDirReport::SteerDirAuto;
    Velocity vel = traj_pointer_->VelocityForSteerAngle(STEERING_VEL_THRESHOLD, steer_dir);
    DEBUG_WARN_OUT("steer is not ok, to DoSteerCmd and SetAutoModeNavVel");
    DoSteerCmd(vel, steer_dir, true); // 此处待优化
    SetAutoModeNavVel(vel.vel_x_, vel.vel_y_, vel.angular_);
    is_steering_ = true;
    DEBUG_WARN_OUT("bSteering is running");
    if (FinishSteerCmd()) {
        ret = true;
        is_steering_ = false;
        DEBUG_WARN_OUT("OK;FinishSteerCmd:set steering false");
    }

    return ret;
}

bool TrajNv::IsStop(short traj_type) 
{
    bool spin_traj = traj_type == (short)TrajType::SPIN_TRAJ;
    bool velocity_zero = fabs(velocity_.linear_) <= STOP_VEL_THRESHOLD;
    bool angular_velocity_zero = fabs(velocity_.angular_) <= STOP_VEL_THRESHOLD;
    bool stop_agv = spin_traj ? (angular_velocity_zero && velocity_zero) : velocity_zero;
    return stop_agv;
}

/**
 * @brief 根据轨迹类型决定是否减速
 *
 * 根据轨迹类型、置信度、误差等因素决定是否减速
 *
 * @param traj_type 轨迹类型
 * @return bool 是否需要减速
 */
bool TrajNv::ProfilerRefrain(short traj_type)
{
#ifdef AGV_DEBUG_NAV
    //测试使用，事件缓停
    uint8_t mode = EventOccur();
    bool slow_stop = (mode == agv_msgs::msg::AGVEvent::SLOWDONE);
    return slow_stop;
#endif
    DEBUG_OUT("ProfilerRefrain:get pose start");
    uint8_t control_mode = EventOccur();
    bool is_slow_down = (control_mode == agv_msgs::msg::AGVEvent::SLOWDONE);
    // 置信度过低减速(<20)
    float x = 0.0f, y = 0.0f, theta = 0.0f, quality = 0.0f;
    bool quality_failed = !(GetLaserAutoPos(x, y, theta, quality, false, false) && quality > 20.0f);
    // 误差较大，速度分割处理
    float deviation = fabs(guide_helper_.GetProjectData().x_);
    float ang_deviation = fabs(Angle(guide_helper_.GetProjectData().theta_).NormAngle2());

    float bezier_limit_y = MotionParm::refrain_params_.y_limit_bezier_;
    float bezier_limit_ang = MotionParm::refrain_params_.angle_limit_bezier_;
    float line_limit_y = MotionParm::refrain_params_.y_limit_line_;
    float line_limit_ang = MotionParm::refrain_params_.angle_limit_line_;

    float deviation_limit = traj_type == (short)TrajType::BEZIER_TRAJ ? bezier_limit_y : line_limit_y;                          // 4cm 6cm
    float deviation_limit_ang = traj_type == (short)TrajType::BEZIER_TRAJ ? TO_RADIAN(bezier_limit_ang) : TO_RADIAN(line_limit_ang); // 0.122 : 0.174f; // 5degree 7degree

    bool is_dev_trigger = deviation > deviation_limit;
    bool is_ang_trigger = ang_deviation > deviation_limit_ang;
    bool need_slow_down = is_ang_trigger || is_dev_trigger || quality_failed || is_slow_down;
 
    if (need_slow_down) {
        DEBUG_OUT("force set low speed:"
                  << ",is_dev_trigger:" << is_dev_trigger
                  << ",is_ang_trigger:" << is_ang_trigger
                  << ",deviation=" << deviation
                  << ",ang_deviation=" << ang_deviation
                  << ",deviation_limit=" << deviation_limit
                  << ",deviation_limit_ang=" << deviation_limit_ang
                  << ",is_slow_down=" << is_slow_down
                  << ",quality_failed=" << quality_failed);
    }

    return need_slow_down;
}

/**
 * @brief 获取旋转节点对象
 *
 * 获取TrajNv对象中存储的旋转节点对象指针。
 *
 * @return 指向旋转节点对象的指针，如果不存在则返回NULL。
 */
Node *TrajNv::GetSpinNodeObj()
{
    Trajectory *traj_point = GetTraj();
    if (!traj_point) {
        DEBUG_ERROR_OUT("GetSpinNodeObj: pTraj is null");
        return NULL;
    }

    bool is_spin_type = (short)TrajType::SPIN_TRAJ == traj_point->GetType();
    if (!is_spin_type) {
        return nullptr;
    }

    return spin_node_pointer_;
}

/**
 * @brief 生成进度和角度
 *
 * 根据轨迹指针对象的当前状态，生成进度速度角度
 */
void TrajNv::GenerateProgVelSteerAngle()
{
    if (!VerifyTrajObj()) {
        return;
    }

    // 1:Get planning speed and remaining distance
    rate_ = profiler_.GetCurValue();             // change  ;current ctrol vel
    area_ = profiler_.GetCurArea();              // use
    real_progress_ = traj_pointer_->GetRange() - area_; // current run distance

    if (real_progress_ < 0)
        real_progress_ = 0;
    else if (real_progress_ > traj_pointer_->GetRange())
        real_progress_ = traj_pointer_->GetRange();

    DEBUG_OUT("GenerateProgVelSteerAngle:"
              << "m_fRate=" << rate_
              << ",m_fArea=" << area_
              << ",Range=" << traj_pointer_->GetRange()
              << ",m_fRelProgress=" << real_progress_);


    // 2:important
    // 1 set track speed + progress
    // 2 obtain the rudder angle of the trajectory
    // 3.Get the velocity vector generated by the trajectory
    // 当急停时，如果速度和进度都不变，bezier曲线可能出现t递增问题
    traj_pointer_->SetRelProgress(rate_, real_progress_);
    steer_angle_ = traj_pointer_->SteeringFun(); // shiftPattern: steer need to m_angSteer
    velocity_ = traj_pointer_->VelocityFun();

    // 3:Other
    // If the initial pose is unknown, the trajectory generates the pose
    if (!init_posture_known_) {
        TrajNv::static_posture_ = traj_pointer_->PostureFun();
        TrajNv::init_posture_known_ = TRUE;
    }
}

/**
 * @brief 完成导航任务后的处理函数
 *
 * 完成导航任务后，调用此函数进行后续处理。
 *
 * 将完成标志设置为true，并更新存储AGV的静态位置信息。
 *
 */
void TrajNv::OnFinish()
{ 
    finished_ = true; 
    VhcStatus.StaticLocation(start_node_id_, end_node_id_, true, false); // After the section is completed, set the static attitude of the storage trolley
    DEBUG_OUT("Arrive at node " << end_node_id_ << ", current path segment finished");
}
